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Abstract.  Unmanned  ground  vehicles  have  important  applications  in  high  speed,  rough 
terrain  scenarios.  In  these  scenarios  unexpected  and  dangerous  situations  can  occur  that 
require  rapid  hazard  avoidance  maneuvers.  At  high  speeds,  there  is  limited  time  to 
perform  navigation  and  hazard  avoidance  calculations  based  on  detailed  vehicle  and 
terrain  models.  This  paper  presents  a  method  for  high  speed  hazard  avoidance  based  on 
the  “trajectory  space,”  which  is  a  compact  model-based  representation  of  a  robot’s 
dynamic  performance  limits  in  rough,  natural  terrain.  Simulation  and  experimental 
results  on  a  small  gasoline-powered  unmanned  ground  vehicle  demonstrate  the  method’s 
effectiveness  on  sloped  and  rough  terrain. 

1.  Introduction  and  Background  Literature 

Many  important  military,  disaster  relief,  search  and  rescue,  and  surveillance 
applications  would  benefit  from  an  unmanned  ground  vehicle  (UGV)  capable  of  moving 
safely  at  high  speeds  through  rough,  natural  terrain  of  varying  compositions  and  physical 
parameters  (Gerhart,  1999),  (Walker  2001).  High-speed  UGVs  can  reach  their  target 
location  in  less  time  than  their  low-speed  counterparts,  increase  reconnaissance 
effectiveness  by  traversing  more  terrain  in  a  fixed  amount  of  time,  and  spend  less  time  in 
unsafe  situations. 

Often  a  UGV  is  directed  to  follow  a  pre-planned  route  (or  navigate  among  pre-defined 
waypoints)  generated  by  an  off-line  planning  algorithm  using  coarse  topographical  map 
data.  In  natural  terrain  at  high  speeds,  dangerous  situations  unforeseen  by  the  high-level 
planner  are  likely  to  occur  that  require  a  UGV  to  quickly  execute  an  emergency 
maneuver.  These  may  be  the  result  of  outdated  topographical  map  data,  unidentified 
hazards  due  to  sensor  limitations  or  errors,  or  unanticipated  physical  terrain  conditions. 
Despite  ever  increasing  computing  power,  at  high  speeds  there  is  little  time  to  accurately 
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model  complex  and  detailed  vehicle/terrain  interactions  in  real  time  and  perform 
navigation  based  on  these  models. 

This  paper  presents  a  local  replanning  algorithm  for  emergency  hazard  avoidance 
situations  applicable  to  UGVs  traveling  at  high  speed  on  rough  natural  terrain.  Note  that 
“high  speed”  is  a  function  of  vehicle  geometry  and  terrain  and  is  loosely  defined  here  as 
speeds  that  excite  vehicle  dynamic  effects  such  as  rollover,  ballistic  motion,  sideslip,  and 
wheel  slip.  The  proposed  algorithm  considers  the  effects  of  terrain  inclination, 
roughness,  and  traction.  In  addition,  it  generates  dynamically  feasible  paths  by 
construction  and  is  computationally  efficient.  The  paper  does  not  focus  on  path 
following  or  sensing,  although  they  are  recognized  as  important  and  necessary 
components  for  a  comprehensive  unmanned  ground  vehicle. 

1.1.  Problem  Formulation  and  Key  Assumptions 

In  this  paper  a  UGV  is  assumed  to  be  following  a  nominal  (pre-planned)  path, 
x»w„m„/(s  )  =  (^G  ))’(v)),  ve  [v0,.y;  ].  Associated  with  xnominai  is  a  nominal  trajectory 
comprised  of  a  vehicle’s  desired  velocity  and  path  curvature,  Tnomjnal(s)=  (v^)?^)). 
There  exists  a  unique  mapping  from  a  vehicle’s  trajectory  to  its  path  given  the  vehicle’s 
initial  curvature,  heading,  and  position. 

Hazards  are  defined  as  discrete  objects  or  terrain  features  that  significantly  impede  or 
halt  UGV  motion,  such  as  trees,  boulders,  ditches,  knolls,  and  areas  of  poorly  traversable 
terrain  (e.g.  water  or  very  soft  soil).  Hazards  are  assumed  to  be  detected  by  on-board 
range  sensors.  It  is  recognized  that  hazard  detection  and  sensing  are  important  aspects  of 
UGV  mobility  and  an  active  research  topic  (Fish,  2003),  (Shoemaker  &  Borenstein, 
2000);  however,  it  is  not  a  focus  of  this  work. 

A  terrain  patch  is  described  by  its  average  roll  (0),  pitch  (»//),  roughness  (f>  ),  and 
traction  coefficient  (u  ).  It  is  assumed  that  coarse  estimates  of  the  tire/ground  traction 
coefficient  and  ground  roughness  are  known  or  can  be  determined  online  using  currently 
available  techniques  (Arakawa  &  Krotkov,  1993),  (Iagnemma,  Kang,  Brooks,  & 
Dubowsky,  2003),  (Manduchi,  Castano,  Talukder,  &  Matthies,  2005). 

The  vehicle  is  assumed  to  be  equipped  with  a  forward-looking  range  sensor  that  can 
measure  terrain  elevation  and  locate  hazards  up  to  several  vehicle  lengths  ahead;  an 


2 


Accepted  to  the  Journal  of  Field  Robotics 


inertial  navigation  sensor  that  can  measure  the  vehicle’s  roll,  pitch,  yaw,  roll  rates,  pitch 
rates,  yaw  rates,  and  translational  accelerations  with  reasonable  uncertainty;  and  a  global 
positioning  system  that  can  measure  the  vehicle’s  position  and  velocity  in  space  with 
reasonable  uncertainty. 

1.2.  Background  Literature  Review 

Hazard  avoidance  for  UGVs  has  been  traditionally  performed  by  selecting  from  a  set 
of  candidate  paths  (i.e.  search  techniques  over  small  spaces)  or  through  the  use  of 
reactive  (reflexive)  behaviors.  Other  proposed  methods  of  interest  include  potential 
fields,  mixed-integer-linear  programming,  the  curvature-velocity  method  (Simmons, 
1996),  and  the  dynamic  window  approach  (Fox,  Burgard,  &  Thrun,  1997).  Many  of  the 
techniques  have  been  designed  for  use  on  flat  or  slightly  rolling  terrain  at  speeds  that  do 
not  excite  the  vehicle  dynamics.  This  paper  addresses  hazard  avoidance  on  flat,  rough, 
and  uneven  terrain  at  speeds  that  excite  vehicle  dynamics. 

Previous  researchers  have  addressed  this  problem  with  a  search-based  technique  to 
navigate  a  HMMWV-class  vehicle  at  speeds  up  to  10  m/s  while  avoiding  large  hazards 
(Coombs,  Lacaze,  Legowik,  &  Murphy,  2000).  The  method  relies  on  a  pre-computed 

n 

database  of  approximately  1.5x10  clothoidal  paths.  Since  the  vehicle  is  assumed  to 
travel  on  relatively  flat  terrain  at  fairly  low  speeds,  the  model  used  in  the  calculations 
does  not  consider  vehicle  dynamics.  An  online  algorithm  eliminates  candidate  clothoids 
that  intersect  with  hazards  or  are  not  feasible  given  the  initial  steering  conditions.  From 
the  remaining  path,  the  algorithm  chooses  one  that  follows  the  most  benign  terrain. 
Several  contenders  of  the  2005  DARPA  Grand  Challenge  utilize  similar  approaches 
which  have  proven  to  be  successful  for  speeds  in  excess  of  8  m/s.  However,  the 
techniques  do  not  consider  the  important  aspects  of  terrain  roughness,  inclination,  and 
vehicle/terrain  traction  characteristics,  all  of  which  will  become  increasingly  more 
important  as  autonomous  vehicles  move  from  traversing  roads  and  relatively  benign 
terrain  to  more  dangerous  and  extreme  topography. 

In  another  successful  technique,  a  UGV  uses  a  voting  scheme  to  evaluate  several 
forward  simulated  candidate  paths  through  a  terrain  region  (Kelly  &  Stentz,  1998).  An 
arbitrator  chooses  the  path  closest  to  the  goal  location  that  does  not  impact  a  hazard.  This 
method  has  been  experimentally  demonstrated  on  a  HMMWV-class  vehicle  at  speeds  up 
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to  4.5  m/s  on  natural  terrain.  This  work  employs  kinematic  UGV  models  to  evaluate 
potential  paths,  and  does  not  account  for  dynamic  vehicle  effects  such  as  rollover  and 
sideslip.  An  extension  of  this  work  proposes  to  limit  the  set  of  initial  candidate  paths  by 
placing  constraints  on  the  space  of  a  vehicle’s  velocity  and  curvature  (Sanjiv  &  Kelly, 
1996).  This  is  similar  in  some  ways  to  the  method  presented  in  this  paper;  however,  this 
methodology  again  does  not  consider  dynamic  effects  that  can  be  important  at  high 
speeds. 

Another  class  of  techniques  applied  to  mobile  robot  navigation  in  natural  terrain  relies 
on  reactive  behaviors  that  generate  a  specific  action  in  response  to  online  sensor  signals 
(Brooks,  1986).  A  successful  reactive  behavior  technique  for  outdoor  hazard  avoidance 
arbitrates  between  hazard  avoidance  and  goal  seeking  and  allows  for  UGV  navigation  at 
speeds  of  up  to  1  m/s  (Daily  et  al.,  1998),  (Olin  &  Tseng,  1991).  Another  approach 
defined  five  candidate  behaviors  that  “voted”  for  or  against  a  set  of  steering  angles: 
“avoid  obstacles,”  “follow  the  road,”  “seek  the  goal,”  “maintain  heading,”  and  “track  the 
path”  (Langer,  Rosenblatt,  &  Hebert,  1994).  The  steering  angle  with  the  most  votes  was 
then  executed.  Although  both  these  techniques  have  been  successful  at  low  to  moderate 
speeds,  none  explicitly  consider  vehicle  dynamics  and  terrain  characteristics  which  can 
result  in  trajectories  that  are  impossible  for  a  UGV  to  safely  execute  at  high  speeds  on 
rough  terrain. 

Potential  field  methods  have  also  been  applied  to  mobile  ground  robots  in  natural 
terrain  (Chanclou  &  Luciani,  1996),  (Haddad,  Khatib,  Lacroix,  &  Chatila,  1998).  These 
methods  define  a  potential  field  in  Cartesian  space  with  goal  locations  corresponding  to 
“sink”  functions  and  hazard  locations  corresponding  to  “source”  functions.  A  vehicle 
then  navigates  towards  points  of  increasingly  low  potential.  These  techniques  have 
proven  successful  for  slow  moving  UGVs  on  flat  terrain;  however,  they  do  not  consider 
the  effects  of  robot  dynamics,  terrain  inclination,  roughness,  or  traction.  A  potential  field 
method  has  recently  been  developed  for  high  speed  vehicles  on  rough  terrain  that  does 
consider  these  effects  (Shimoda,  Kuroda,  &  Iagnemma,  2005).  This  work  utilizes  the 
trajectory  space  framework  presented  in  this  paper. 

Two  similar  approaches  of  interest  are  the  curvature-velocity  method  (Simmons, 
1996)  and  the  dynamic  window  technique  (Fox,  Burgard,  &  Thrun,  1997),  (Brock  & 
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Khatib,  1999),  (Philippsen  &  Siegwart,  2003).  Both  consider  the  effects  of  velocity  and 
acceleration  constraints  on  synchro-drive  robots  operating  in  indoor  environments.  A 
two-dimensional  space  consisting  of  a  vehicle’s  translational  and  rotational  velocities  is 
created.  Velocity  combinations  that  cannot  be  reached  due  to  acceleration  constraints 
and  obstacle  locations  are  removed  from  this  space.  A  translational  and  rotational 
velocity  is  then  chosen  from  the  space  by  maximizing  an  objective  function.  Both  the 
curvature- velocity  method  and  dynamic  window  approach  have  similar  elements  to  the 
trajectory  space  method  described  in  this  paper.  However,  they  do  not  consider  critical 
vehicle/terrain  interactions,  vehicle  dynamic  effects,  or  terrain  inclination. 

In  summary,  numerous  techniques  have  been  developed  for  navigation  and  hazard 
avoidance  of  unmanned  ground  vehicles.  In  general,  these  techniques  do  not  explicitly 
consider  dynamic  vehicle  and  terrain  effects  that  are  important  for  UGVs  traversing 
rough  terrain  at  high  speeds.  A  method  that  considers  these  effects  and  is 
computationally  tractable  is  the  objective  of  this  work. 

2.  Trajectory  Space  Description 

The  hazard  avoidance  algorithm  presented  here  is  based  on  the  trajectory  space, 
defined  as  the  two-dimensional  space  of  a  vehicle’s  instantaneous  longitudinal  velocity, 
v,  and  path  curvature,  /c  (Spenko,  Iagnemma,  &  Dubowsky,  2004).  A  UGV’s  “position” 
in  the  trajectory  space  is  a  velocity-curvature  pair  (v,?e).  The  relationship  of  a  point  in 
the  trajectory  space  and  a  vehicle  maneuver  is  illustrated  in  Figure  1.  Velocities  are 
limited  to  positive  values  in  this  work.  A  transition  between  two  points  in  the  space  can 
be  thought  of  as  a  simple  maneuver. 
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Figure  1.  Representation  of  vehicle  action  as  described  by  coordinates  in  the  trajectory 

space. 

The  trajectory  space  is  a  useful  space  for  UGV  navigation  for  two  reasons.  First, 
points  in  the  trajectory  space  easily  map  to  the  points  in  UGV  actuation  space  (generally 
consisting  of  one  throttle/brake  control  input  and  one  steering  angle  control  input).  Thus 
navigation  algorithms  developed  for  use  in  the  trajectory  space  will  map  to  command 
inputs  that  obey  vehicle  nonholonomic  constraints.  Second,  constraints  related  to  terrain 
parameters  including  inclination,  roughness,  and  traction  and  to  dynamic  effects  such  as 
UGV  rollover  and  sideslip  are  easily  expressible  in  the  trajectory  space,  since  these 
effects  are  strong  functions  of  velocity  and  path  curvature. 

Sections  2. 1-2.2  describe  computation  of  these  constraints  using  simple  UGV  and 
terrain  models.  These  computationally  efficient,  low-order  models  have  been  shown  to 
sufficiently  capture  the  important  vehicle  dynamics  and  vehicle/terrain  interactions. 

2.1.  The  Dynamic  Trajectory  Space 

The  dynamic  trajectory  space  is  a  subspace  of  the  trajectory  space  that  consists  of 
velocity-curvature  pairs  that  correspond  to  dynamically  feasible  maneuvers  on  a  given 
terrain  patch.  Dynamically  feasible  maneuvers  are  here  defined  as  maneuvers  that  do  not 
self-induce  vehicle  failure  due  to  excessive  sideslip  (i.e.  skidding)  or  rollover  and  are 
physically  attainable  considering  vehicle  steering  properties. 
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2.1.1.  Sideslip  Constraint  Computation 

Although  some  sideslip  is  expected  and  likely  unavoidable,  substantial  slip  that  causes 
large  heading  or  path  following  errors  is  detrimental.  A  constraint  function  relating  UGV 
velocity  and  path  curvature  to  lateral  traction  limits  can  be  derived  from  the  free  body 
diagram  shown  in  Figure  2,  assuming  v  =  0  and  k-  0.  Here  {xyz}  represents  a  body- 
fixed  coordinate  frame  and  {XYZ}  represents  an  inertial  frame. 


i-mv  +mgx 


Figure  2.  Sideslip  vehicle  model. 

For  simplicity  vehicle  roll  and  pitch  are  assumed  to  be  equal  to  the  roll  and  pitch  of  the 
terrain  beneath  the  vehicle.  Hence,  suspension  travel  is  neglected.  The  vehicle  roll  (0), 
pitch  (y/),  and  yaw  (0)  associated  transformation  matrices  are: 
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The  acceleration  due  to  gravity  in  {xyz}  following  a  roll,  pitch,  yaw  transformation  is 
thus:  Sxyz  =G/vGegxYz>  where  Sxyz  =  fcx  Sy  8  J  A  vehicle  begins  to  skid  when 
the  traction  force  is  equal  to  the  sum  of  the  centripetal  and  gravitational  force 
components.  In  this  simplified  analysis,  accelerations  other  than  gravity  in  the  z- 
direction  are  ignored.  The  predicted  maximum  curvature  before  skidding  occurs  can  be 
computed  as: 


K 


min,  max 


—  2  + 


gx  ±  m. 


slip 


(2) 


The  two  solutions  correspond  to  downslope/upslope  travel.  Treating  Equation  2  as  a 
constraint  function,  the  sideslip  space  is  defined  as: 

Definition  1  (Sideslip  Space)  The  sideslip  space,  A,  is  the  set  of  velocity  and  curvature 
pairs  that  do  not  induce  vehicle  sideslip  on  a  given  terrain  patch: 
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Figure  3(a)  illustrates  the  variation  of  the  sideslip  constraint  as  a  function  of  traction 
coefficient.  As  expected,  as  the  traction  coefficient  increases  (at  a  fixed  speed)  a  UGV 
can  safely  execute  maneuvers  of  increasing  curvature.  Figure  3(b)  illustrates  the 
variation  of  the  sideslip  constraint  as  a  function  of  terrain  inclination.  The  sloped  terrain 
constraint  corresponds  to  a  UGV  traversing  a  side  slope  of  30°  with  the  fall  line 
perpendicular  to  the  vehicle’s  heading  (negative  curvatures  represent  a  downslope  turn). 
As  expected  the  UGV  can  safely  execute  a  downslope  turn  at  higher  velocity  than  it  can 
execute  an  upslope  turn,  as  a  result  of  the  interaction  of  centripetal  acceleration  and 
gravity. 


Clearly  the  sideslip  model  presented  here  does  not  include  potentially  significant 
effects  such  as  load  transfer;  however  it  is  employed  due  to  its  simplicity  and  has  shown 
to  be  reasonably  accurate  in  practice  (Spenko,  2005). 


Figure  3.  Sideslip  constraint  comparison  for  varying  (a)  traction  coefficient  and  (b) 

inclination. 

2.1.2.  Rollover  Constraint  Computation 

Rollover  is  generally  undesirable  despite  the  fact  that  some  UGVs  are  designed  to  be 
invertible  (Walker,  2001).  A  trajectory  space  constraint  for  vehicle  rollover  can  be 
computed  from  analytical  models  of  varying  complexity.  Here  a  rigid  body  model  is 
presented,  although  models  that  consider  the  effects  of  tire  stiffness  and  suspension 
compliance  are  also  available  (Spenko,  2005),  (Chen  &  Peng,  1999). 

Rollover  is  initiated  when  the  body  force  vector  expressed  at  the  vehicle  center  of 
mass  is  directed  outside  the  convex  polygon  formed  by  the  tire/terrain  contact  points.  For 
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a  UGV  with  a  wheelbase  greater  than  its  track  width,  this  most  commonly  occurs  when 
the  moment  about  either  of  the  points  A  in  Figure  2  is  equal  to  zero.  The  resulting 
constraint  function  for  the  rollover  space  is: 


K. 


max, min 
rollover 


dg  ±  hgx 
hv 2 


(4) 


where  d  is  one  half  the  axle  length,  h  is  the  center  of  mass  height,  and  v  is  the  vehicle’s 
longitudinal  velocity.  The  two  solutions  correspond  to  upslope/downslope  travel.  This 
model  has  been  found  experimentally  to  accurately  predict  rollover  on  flat,  smooth 
terrain  (Spenko,  2005).  The  effect  of  terrain  roughness  of  the  accuracy  of  this  model  is 
investigated  in  Section  2.3. 

Definition  2  (Rollover  Space)  The  rollover  space,  B,  is  the  set  of  velocity  and  curvature 
pears  on  a  given  terrain  patch  that  do  not  induce  vehicle  rollover: 

K  i  c, L,  <*<  «sJ  (5) 


Similar  to  Figure  3(b),  the  rollover  constraint  will  vary  as  a  function  of  terrain 
inclination. 


2.1.3.  Steering  Constraint  Computation 

Steering  constraints  describe  the  limitations  imposed  by  a  UGV’s  kinematic  steering 
and  handling  properties  on  its  attainable  maneuvers.  Here  front- steered  vehicles  that  can 
exhibit  neutral-,  over-,  or  understeer  are  considered.  Many  factors  contribute  to 
determining  if  a  vehicle  will  exhibit  oversteer  or  understeer  (Gillespie,  1992).  Tire 
cornering  properties  and  the  location  of  the  center  of  mass  are  major  influences,  and  their 
effects  are  investigated  here. 
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Figure  4.  Single-track  vehicle  model  for  steering  mechanism  limit  computation  (adapted 

from  Gillespie,  1992). 

The  slip  angle,  a,  is  the  angle  between  the  heading  direction  of  the  tire  and  the 
direction  of  travel,  (see  Figure  4).  At  low  slip  angles  (typically  a  <  5°),  the  relationship 
between  the  cornering  force  and  slip  angles  is  approximately  linear  (Gillespie,  1992): 

Ff  r  -  Ckaf  r  (6) 

where  C*  is  the  cornering  stiffness  of  the  tire.  Summing  the  lateral  forces  acting  on  the 
vehicle  results  in: 

Ff  cos  8  +  Fr  -  mv2K  ±  mg  x  (7) 


Summing  the  moments  about  the  center  of  mass  yields: 

FA,  cos  8  =  Frlr 


Combining  Equations  6,  7,  and  8  and  linearizing  about  <5  =  0  yields: 


a 


m(y2K±gx) 


f,r 


c, 


f  l  ^ 

Kf 

v  J  y 


By  examining  Figure  4  it  is  evident  that: 


8  =  LK  +  af  -a 


(8) 


(9) 


(10) 


Combining  Equations  9,  and  10  and  yields  a  constraint  function  for  the  steering  space: 


CkF8m.dX±mg  {[f -l  ) 
CkL2  +  mv2(lr  -  If) 


(ID 
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Definition  3  (Steering  Space)  The  steering  space,  C,  is  the  set  of  velocity  and  cun’ature 
pairs  that  are  attainable  given  a  vehicle’s  kinematic  configuration  and  tire  parameters: 

c(p,yf,n,v,m, Ck,lf  r )=  %k\  C„s  <*'<  <Z,„,  J  (12) 

Figure  5  illustrates  the  difference  in  steering  constraints  for  generic  understeered  and 
oversteered  vehicles. 


Figure  5.  Illustration  of  comparison  of  steering  limits  for  over/understeered  vehicles. 

2.1.4.  Drive  Train  Constraint  Computation 

Generally  a  UGV’s  maximum  forward  velocity  is  a  function  of  the  drive  train 
characteristics,  rolling  resistance,  aerodynamic  drag  effects,  and  terrain  inclination.  Here 
a  trajectory  space  constraint  related  to  maximum  forward  velocity  is  computed  from  the 
model  in  Figure  6. 


Figure  6.  Simple  drive  train  vehicle  model. 

For  this  system,  summing  the  forces  in  the  y-direction  yields: 

m'y  =  F powertrain  ~  F drag  ~  F rolling  ~  ™S  (13) 

A  model  of  the  force  generated  by  a  general  UGV  power  train  can  be  written  as: 
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_T{v)0 


powertrain 


(14) 


where  T (v )  is  the  torque,  G  is  the  transmission  ratio,  and  r  is  the  wheel  radius.  The 
aerodynamic  drag  force  on  a  vehicle  can  be  modeled  by  the  relation  (White,  1994): 


=\p\cM 


(15) 


where  p  is  the  density  of  air,  Ar  is  the  reference  area  of  the  vehicle,  and  Q  is  the  drag 
coefficient.  The  rolling  resistance  acting  on  a  wheeled  vehicle  can  be  modeled  by  the 
relation  (Beer  &  Johnston,  1988): 

F rolling  =crrmg  cos  (p  (16) 


where  Crr  is  the  coefficient  of  rolling  resistance.  Combining  Equations  15,  and  16  and 
setting  y  =  0  yields  the  maximum  vehicle  forward  velocity,  which  can  be  treated  as  a 
constraint  function  on  the  drive  train  space: 


vm„  = 


_  2  (r  (v  -  rCnmg  cos  (p-  ring  sin  (p  ) 


rpCd 


(17) 


Definition  4  (Drive  Train  Space)  The  drive  train  space,  D,  is  the  set  of  velocity  and 
curx’ature  pairs  that  are  attainable  on  a  given  terrain  patch  given  a  vehicle ’s  drive  train 
and  aerodynamic  properties: 

Dfo,y/,m,p,K)=fyv\0<v<vmax}  (18) 


where  vmax  is  the  maximum  vehicle  velocity. 

The  sideslip  space,  rollover  space,  steering  space,  and  drive  train  space  are  subspaces 
of  the  trajectory  space  constrained  by  various  functions  related  to  UGV  mobility. 
Together  they  comprise  the  dynamic  trajectory  space. 

Definition  5  (Dynamic  Trajectory  Space)  The  dynamic  trajectory  space,  T,  is  the  set  of 
velocity  and  curx’ature  pairs  that  are  attainable  on  a  given  terrain  patch  given  a  vehicle ’s 
dynamic,  steering,  and  drive  train  properties: 

r^AnBflCflD  (19) 
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Figure  7  shows  an  illustration  of  the  dynamic  trajectory  space.  The  dynamic  trajectory 
space  represents  all  kinematically  and  dynamically  feasible  velocity-curvature  pairs  for  a 
particular  vehicle  on  a  particular  terrain  patch. 


Figure  7.  Dynamic  trajectory  space  (shaded  region)  illustration  for  an  oversteered 

vehicle  on  flat  terrain. 

2.2.  The  Hazard  Trajectory  Space 

In  Section  2.1  models  were  presented  for  computing  trajectory  space  constraints  that 
limit  the  space  of  maneuvers  that  are  safely  attainable  on  a  given  terrain  patch  at  a  given 
instant.  Physical  hazards  (such  as  large  rocks,  trees,  deep  water,  etc.)  also  limit  the  space 
of  safely  attainable  maneuvers. 

Hazards  can  be  generally  classified  as  belonging  to  one  of  two  types:  trajectory 
independent  hazards  and  trajectory  dependent  hazards.  A  trajectory  independent  hazard, 
such  as  a  large  tree,  boulder,  or  water  trap,  is  one  that  a  vehicle  cannot  safely  travel 
across,  over,  or  through  independent  of  approach  velocity  and  direction.  A  trajectory 
dependent  hazard  is  one  where  safe  traversal  depends  on  the  vehicle  approach  velocity 
and/or  direction,  such  as  a  shallow  ditch  where  at  high  velocities  a  UGV  may  be  able  to 
achieve  ballistic  motion  and  successfully  “jump”  the  ditch. 

The  hazard  trajectory  space  is  defined  as  the  space  of  velocity-curvature  pairs  that,  at 
the  current  UGV  position,  would  result  in  a  collision  between  the  vehicle  and  hazard. 
For  a  trajectory  independent  hazard,  curvatures  between  tc'jjjard  and  in  Figure  8 

would  thus  belong  to  the  hazard  trajectory  space.  The  minimum  and  maximum 
curvatures  associated  with  a  hazard,  KjjjuJ  and  k',."'"„yJ  ,  are  generally  functions  of  a 
vehicle’s  velocity  due  to  the  fact  that  vehicles  follow  clothoidal  paths,  which  correspond 
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to  the  steering  angle  changing  with  constant  velocity,  when  transitioning  between 
curvatures  (Spenko,  2005).  An  illustration  of  the  hazard  trajectory  space  for  a  trajectory 
independent  hazard  is  shown  in  Figure  9(a).  An  illustration  of  the  hazard  trajectory  space 
for  a  trajectory  dependent  hazard  is  shown  in  Figure  9(b). 


Figure  8.  Illustration  of  maximum  and  minimum  curvatures  necessary  to  avoid  impact 

with  a  hazard. 

Definition  6  (Hazard  Trajectory  Space)  The  hazard  trajectory  space,  LI,  consists  of 
curx’atures  and  velocities  that,  if  maintained  from  the  current  vehicle  position,  would  lead 
to  intersection  with  a  hazard. 

)-  F(v,  KT  )l  C  £  V  £  K'Z,d  <  K  <  KZ„  )  (20) 


(b)  trajectory  dependent  hazard. 


Note  that  there  are  no  limitations  as  to  the  number  of  hazards  that  can  appear  in  the 
trajectory  space.  The  hazard  trajectory  space  is  generated  by  evaluating  a  pre-computed 
library  of  clothoidal  paths  that  connect  the  current  location  in  the  trajectory  space  to  other 
locations. 
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2.3.  Effect  of  Roughness  on  Trajectory  Space  Constraints 

The  above  models  of  UGV  mobility  have  assumed  that  the  terrain  is  smooth.  Terrain 
roughness  influences  UGV  mobility  by  inducing  variation  in  the  wheel  normal  forces, 
which  affects  cornering  and  stability  properties.  The  effect  of  terrain  roughness  on  UGV 
sideslip  and  rollover  has  been  studied  in  simulation  (Spenko,  2005).  Representative 
results  for  an  analysis  of  UGV  rollover  are  shown  in  Figure  10. 


It  can  be  observed  that  as  terrain  roughness  increases  (represented  by  an  increasing 
fractal  number),  the  variation  in  velocity  and  curvature  at  which  rollover  is  initiated 
increases.  In  general,  terrain  roughness  tends  to  induce  variation  in  the  location  of 
trajectory  space  constraints.  Based  on  this  observation,  researchers  have  suggested  that  a 
probabilistic  representation  of  vehicle  mobility  is  more  useful  analysis  tool  than  a 
deterministic  (i.e.  “go/no-go”)  analysis  (Golda,  2003).  A  more  thorough  discussion  of 
the  effect  of  terrain  roughness  on  trajectory  space  constraints  can  be  found  in  (Spenko, 
2005). 


2.5  Fractal  Terrain 


0[ - 1 - 1 - 1 - 1 - 1 - 1 - 1 - 1 

4  4.5  5  5.5  6  6.5  7  7.5  8 

Velocity  (m/s) 

Figure  10.  Comparison  of  rollover  constraints  on  smooth  and  rough  terrain. 


2.4.  The  Admissible  Trajectory  Space 


Not  all  points  in  the  trajectory  space  can  be  reached,  or  transitioned  to,  from  another 
point  in  a  given  time  t,  due  to  limits  on  UGV  acceleration,  deceleration,  and  steering  rate. 
Thus  not  all  maneuvers  are  physically  realizable.  Knowledge  of  the  set  of  reachable 
maneuvers  is  important  during  navigation,  since  UGVs  often  have  a  limited  amount  of 
time  to  react  to  impending  hazards. 
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A  maneuver  is  defined  as  a  transition  between  two  arbitrary  velocity-curvature  pairs. 
A  maneuver  is  characterized  by  an  initial  trajectory,  T0  ={y0,K0),  a  final  trajectory, 
r f  =^f,Kf  ),  and  a  distance  5  over  which  the  maneuver  occurs  and  can  be  described  by 
the  quintuple  %  =^0,vf,K0,Kf,s  J.  A  maneuver  can  thus  be  defined  as: 


Definition  7  (Maneuver)  A  maneuver,  %,  is  a  transition  of  any  general  shape  from  one 
location  in  the  trajectory  space  T0  ={y0,K0)  to  another,  T,  =^f,Kf  )  over  a  distance  s. 

For  a  UGV  with  a  location  inside  the  trajectory  space  of  (y0,K0),  the  maximum 
attainable  velocity  in  time  t  is: 


max,  min 
v  reachable 


=  v0  ±  at 


(21) 


where  a  is  the  (assumed  to  be  constant)  acceleration/deceleration  parameter.  The 
maximum  and  minimum  attainable  curvatures  for  a  front-steered  vehicle  in  time  t  are: 

CZ(')=' o±*_<  (22) 


where  7Cmax  is  the  maximum  rate  of  change  of  curvature,  given  as  (see  Figure  4): 

tan  (5, . . 


(23) 


where  <5max  is  the  maximum  steering  rate  of  change  and  is  assumed  to  be  constant. 

The  reachable  trajectory  space,  A,  which  describes  a  space  of  physically  attainable 
maneuvers,  can  then  be  defined: 


Definition  8  (Reachable  Trajectory  Space)  The  reachable  trajectory  space,  A,  is  the 
set  of  admissible  velocity  and  curvature  pairs  a  vehicle  can  transition  to  in  a  given  time  t 
from  an  initial  location  (v0,/c0). 

The  admissible  trajectory  can  now  be  defined.  An  example  is  shown  in  Figure  11. 

Definition  9  (Admissible  Trajectory  Space)  The  admissible  trajectory  space,  0,  is  the 
space  of  all  dynamically  admissible  velocity  and  curx’ature  pairs  on  a  given  terrain  patch 
that  can  be  transitioned  to  in  a  given  time  t  from  an  initial  location  ( v0,K0 )  given  a 
vehicle’s  dynamic,  steering,  and  drive  train  properties: 

0  =  rflA  (24) 
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Figure  11.  Admissible  trajectory  space  for  a  HMMWV-class  vehicle. 

3.  Hazard  Avoidance  and  Path  Resumption  Algorithm  Descriptions 

The  trajectory  space  framework  presented  in  Section  2  yields  an  instantaneous 
description  of  a  UGV’s  mobility  properties  on  a  given  terrain  patch.  During  high  speed 
navigation,  however,  a  hazard  avoidance  algorithm  must  predicatively  analyze  mobility 
over  terrain  in  front  of  the  UGV.  In  this  section  a  hazard  avoidance  algorithm  is 
presented  that  employs  predictive  analysis  of  UGV  mobility  over  local  terrain  using  the 
trajectory  space  framework  presented  in  Section  2. 

Consider  a  scenario  similar  to  that  illustrated  in  Figure  12.  Here,  a  UGV  attempts  to 
follow  a  pre-planned  nominal  desired  path,  xnomina, ,  given  by  a  high-level  path  planner. 
As  discussed  in  Section  1.1,  the  nominal  desired  path  has  a  corresponding  nominal 
desired  trajectory,  Tngminal .  An  on-board  forward-looking  range  sensor  measures 
elevation  and  detects  hazards  in  the  local  terrain  region,  which  is  divided  into  discrete 
patches.  Calculation  of  the  appropriate  size  and  number  of  these  patches  is  an  important 
issue,  but  is  not  discussed  here  (Spenko,  2005).  Note  that  by  separating  the  terrain  into 
patches,  it  is  possible  to  account  for  the  differences  in  terrain  profile  that  will  exist  in  the 
current  sensor  scan.  In  general,  the  size  and  number  of  terrain  patches  is  related  to  the 
desired  accuracy  of  the  terrain  inclination  estimation  used  to  compute  the  total  admissible 
trajectory  space.  As  the  number  of  terrain  patches  increases,  the  estimated  terrain  roll 
and  pitch  for  each  patch  becomes  more  accurate. 
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Figure  12.  Nominal  desired  path  with  terrain  patches. 

A  hazard  avoidance  maneuver  is  enacted  if  either  (or  both)  of  two  situations  occurs:  1) 
A  hazard  lies  on  the  vehicle’s  current  desired  path,  xnominai;  2)  An  element  of  % nominal 
whose  corresponding  path,  xnominai,  lies  in  the  z-th  terrain  patch  violates  a  trajectory  space 
constraint  corresponding  to  that  patch.  In  other  words,  a  UGV  is  commanded  to  follow  a 
dynamically  inadmissible  trajectory. 

At  each  sampling  instant  the  hazard  avoidance  algorithm  can  be  outlined  as  follows: 

1)  Acquire  range  sensor  scan  of  surrounding  terrain  and  discretize  terrain  into  m  patches; 

2)  Compute  trajectory  space  constraints  (Equations  3,  5,  12,  and  18)  of  every  terrain 
patch; 

3)  If  a  hazard  lies  on  the  vehicle’s  current  desired  path,  xnominai  or  an  element  of  t, nominal 
whose  corresponding  path,  xnominai  lies  in  the  z'th  terrain  patch  violates  a  trajectory 
space  constraint  corresponding  to  that  patch,  select  a  hazard  avoidance  maneuver; 

4)  Compute  a  path  resumption  maneuver  that  will  connect  the  end  of  the  path  generated 
by  a  hazard  avoidance  maneuver  to  the  nominal  desired  path. 

Step  1  is  an  important  issue  but  is  not  a  focus  of  this  work.  Step  2  has  been  described 
in  Section  2.  Steps  3-4  are  described  in  the  following  sections. 

3.1.  Hazard  Avoidance  Maneuver  Selection 

Hazard  avoidance  maneuvers  are  dynamically  admissible  modifications  of  a  UGV’s 
nominal  trajectory  initiated  by  the  presence  of  a  hazard  or  dynamically  unsafe  trajectory. 
A  hazard  avoidance  maneuver  is  a  subset  of  the  generalized  maneuver  described  in 
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Definition  7.  It  is  composed  of  two  segments  that  are  truncated  ramps  of  the  highest 
possible  slope.  The  first  consists  of  a  transition  between  the  initial 
trajectory, T0  =(v0,/c0 ),  and  final  trajectory,  r(  =\>f,Kf  J.  The  second  consists  of  a 
constant  velocity  and  curvature  over  a  fixed  distance  (see  Figure  13).  The  total  length  of 
the  maneuver  is  s  =  s  f  -  ,v0 .  Thus,  the  velocity  profile  of  a  hazard  avoidance  maneuver, 
Xh_v,  can  be  described  by: 


Xh 


V 


v0  +  ^(s-s0)  for  s0<s<sv 
as 

vf  for  sv  <  s  <  sf 


(25) 


The  curvature  profile  of  a  hazard  avoidance  maneuver,  Xh_w  can  be  described  by: 

dK , 


Xh. 


K0  H - (5  -  sQ )  for  s0<s  <. 


ds 

K\ 


(26) 


for  sk<s<  sf 


where  so  is  the  initial  starting  point  of  the  maneuver,  sj  is  the  final  point  of  the  maneuver, 
and  sv  and  sK  are  given  as: 


2  2 

^v/-v  0 

Sr=S0±— - -  SK=S0±V 


2  a 


fK1-K^ 

y  ^max  y 


(27) 


where  v  is  the  average  velocity  over  the  time  required  to  reach  the  final  curvature. 


Hazard  Avoidance 

Maneuver  Hazard  Avoidance 


Figure  13.  Hazard  avoidance  maneuver  curvature  (left)  and  velocity  (right)  profiles. 


When  a  hazardous  situation  is  detected,  an  appropriate  hazard  avoidance  maneuver 
must  be  selected.  In  this  work  the  selected  maneuver  must  be  dynamically  admissible 
over  all  local  terrain  patches  (and  not  just  patches  intersecting  xnom(na/)  to  conservatively 
ensure  maneuver  safety. 
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For  a  terrain  region  that  has  been  discretized  into  m  patches,  let  0  ,  denote  the 
admissible  trajectory  space  for  each  terrain  patch,  je  [l,m].  Let  p  be  the  number  of 
hazards  in  the  current  sensor  scan,  and  let  HA  denote  the  hazard  trajectory  space  for  each 
hazard,  k  e  [l,  p\  Then  the  total  admissible  trajectory  space,  Z,  is  defined  as: 

Definition  10  (Total  Admissible  Trajectory  Space)  The  total  admissible  trajectory 
space,  Z,  is  the  intersection  of  admissible  trajectory  spaces  for  every  terrain  patch  minus 
the  hazard  trajectory  space,  H ,  for  each  hazard  in  the  current  sensor  scan: 

Js(ein...n0„,)-H,-H2-...-H,  (28) 

The  total  admissible  trajectory  space  is  a  representation  of  all  safe,  dynamically 
admissible  and  reachable  trajectories  through  a  local  terrain  from  the  UGV’s  current 
position.  The  goal  of  the  hazard  avoidance  algorithm  is  thus  to  find  a  maneuver,  %h, 
such  that  x f  ={yf,Kf  Jb  Z.  This  maneuver  transitions  a  UGV  from  a  point  in  the  total 
admissible  trajectory  space  that  violates  a  constraint  to  one  that  does  not. 

There  are  numerous  techniques  for  finding  xf  that  results  in  a  “good”  maneuver. 
Here  a  simple  and  effective  search-based  method  is  proposed.  In  this  approach,  the 
trajectory  space  is  discretized  into  closely  spaced  points.  This  discretized  trajectory 
space  is  then  treated  as  a  graph,  and  xf  is  chosen  as  the  point  that  minimizes  the  distance 
function  A  from  the  current  point  in  the  trajectory  space,  xQ  =(v0,7f0),  to  a  candidate 
point,  r,  ={yi,Ki ): 

A  =  J  (k,-kJ+-^(v0-vJ  (29) 

V  max  min  v  max 

where  K]  and  K2  are  static  positive  gain  factors.  These  factors  affect  the  relative 
weighting  of  changes  in  velocity  and  curvature.  A  variety  of  search  methods  can  be 
employed  to  find  xf .  In  this  work  an  exhaustive  search  was  utilized  due  the  small  size  of 
the  search  space. 

3.2.  Path  Resumption  Maneuvers 

After  a  hazard  avoidance  maneuver  has  been  computed,  a  UGV  must  plan  a 
kinematically  and  dynamically  feasible  path  to  return  to  the  nominal  desired  path  x, 

Here  an  efficient  path  replanning  algorithm  is  proposed  to  accomplish  this.  There  are 
many  approaches  to  this  problem,  and  a  more  detailed  analysis  of  the  performance  and 
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convergence  properties  of  the  method  along  with  a  comparison  with  other  techniques  is 
described  elsewhere  (Spenko,  2005),  (Kelly  &  Nagy,  2003),  (Reuter,  1998). 

Equations  describing  the  motion  of  a  front-steered  vehicle  can  be  written  as: 

?c(r)=tan(<5(f))/L  d(f)=/c(t}>(f) 
i(f)=v(f)c  os 0 (t )  y(f)=v(t )sin  eh) 


where  8(t )  is  the  steering  input,  6(l)  is  the  vehicle  heading  angle,  v(t)  is  the  velocity 
input,  and  L  is  the  vehicle  wheelbase.  If  the  curvature  input  is  described  as  a  function  of 
time,  u(t),  integration  of  Equation  30  (with  a  change  of  variable  from  time  to  distance, 
and  assuming  constant  velocity)  yields: 


D 

K  (.V  )=u[s)  0  [s  )  — V  J  K  (s  \ls 

0 

D  D 

x  (s  )  =vj  cos  6  (s  )d.s  y(s)  =v  J  sin  6  (s  ):/.s 

0  0 


(31) 


Consider  the  scenario  illustrated  by  the  plot  shown  in  Figure  14.  Here  the  solid  line 
represents  a  nominal  trajectory’s  curvature  as  a  function  of  distance.  A  hazard  avoidance 
maneuver  is  executed  at  sa,  and  the  maneuver  ends  at  .v/,.  The  curvature  profiles  of  the 
nominal  desired  path,  hazard  avoidance  maneuver,  and  path  resumption  maneuver  are 
defined  as  Kj(s),  k'2('s  )’  an<3  KXS)-  These  curvature  profiles  have  associated  heading  and 
position  profiles,  0(.s  ),  x(s),  and  y(s).  The  goal  of  the  path  resumption  problem  is  to 
find  k3  (s')  in  a  computationally  efficient  manner  (i.e.  completed  before  the  UGV  has 
finished  traversing  the  path  generated  by  the  hazard  avoidance  maneuver)  such  that  the 
curvature,  heading,  and  position  at  s(:  is  the  same  as  at  s,i : 

{k (s;  )  0  (sc  )  x (v,  )  3  (s  ))  ={k (sd  )  0  (sd  )  x  (v(/  )  y  {sd  )\  (32) 


where  sc  is  a  desired  “meeting  point”  of  the  replanning  maneuver  and  the  nominal 
trajectory,  and  sci  is  the  terminal  point  of  the  replanning  maneuver.  Clearly  sc  and  s(i  need 
not  be  coincident. 
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Figure  14.  Curvature  diagram  for  path  resumption  maneuver. 

A  path  resumption  maneuver  has  the  following  properties: 

•  An  initial  curvature  and  velocity  that  is  equal  to  the  terminal  curvature  and 
velocity  of  a  hazard  avoidance  maneuver,  ( v[sb )  K  k)\  =14)  K  (Of 

•  A  length  sr  such  that  sr  =  sd  —  sb. 

•  A  trajectory  profile  such  that 

[k  (y  }  0  (v  }  x  (y  )  y  (s  ))  =  [k  {sd  )  0  {sd  )  v  {sd  ][  y  (v(/  )), . 

The  proposed  path  resumption  method,  here  termed  the  “curvature  matching  method,” 
is  outlined  as  follows: 


1.  An  initial  choice  of  the  location  of  the  “meeting  point,”  sc,  on  the  nominal  trajectory 
is  made.  Here  sc  is  initially  chosen  such  that  the  path  length  of  the  path  resumption 
maneuver  is  equal  to  the  path  length  of  the  hazard  avoidance  maneuver: 

sc  =  2sb  ~  sa  (33) 


2.  An  initial  value  of  Sd  is  chosen  to  be  the  smallest  value  such  that  a  vehicle  can 
transition  from  k2 (sy )  to  «3(sd)  without  violating  the  vehicle’s  steering  rate 


constraints: 


k ) 


VK„ 


(34) 


Steps  3-6  are  designed  to  find  a  path  resumption  maneuver  curvature  profile,  k3(s), 
such  that  the  following  condition  is  satisfied: 
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fa  (s)ds  =  jfa  (s)ds  +  jfa  (s)ds 

i  i  i  (35) 

^36  )^  ^max 

where  7Cm,„  and  ?rnMX  are  the  minimum  and  maximum  allowable  curvatures  given  by  the 
dynamic  constraints  of  the  total  admissible  trajectory  space.  Since  Kmjn  and  Kmax  are 
included  in  this  formulation,  the  curvature  matching  method  is  applicable  for  rough  and 
rolling  terrain  because  the  curvature  of  the  resulting  path  will  not  exceed  the  constraints 
generated  by  the  trajectory  space.  Equation  35  states  that  the  area  under  the  hazard 
avoidance  maneuver  curvature  profile  plus  the  area  under  the  path  resumption  maneuver 
curvature  profile  is  equal  to  the  area  under  the  nominal  desired  trajectory  curvature.  This 
ensures  that  the  heading  angle  at  the  end  of  the  path  resumption  maneuver  is  identical  to 
the  heading  angle  of  the  nominal  desired  trajectory  at  the  meeting  point,  0,  (sc  )  =0,  {srl  ). 


3.  Two  curvature  constraints,  Khigh  and  Klow ,  for  the  path  resumption  maneuver  are 
computed  (see  Figure  15).  These  constraints  are  defined  as  the  maximum  and 
minimum  curvatures  that  transition  the  final  curvature  of  the  hazard  avoidance 
maneuver,  K2(sb),  to  the  final  curvature  of  the  path  resumption  maneuver, k3 (sd ), 
given  K  =  /cmax ,  Khigh  <  Kmiu  ,  and  Klow  >  Kmm  .  The  area  between  the  two  constraints 
represents  all  possible  curvatures  that  can  transition  k2  (sh )  to  k2  (sd  ). 


Figure  15.  Illustration  of  curvature  matching  method  constraints. 
Kkigh  and  KU,W  are  given  as: 

:(0) 


^ high 


/  \  dK 
K{sb)+—s 

CIS 

for 

.  ds  ( 

Sb<S<—{Kmax-K^ 

dK 

lv  max 

for 

-y-  (^max  -  Kih,  ))<  '^7L  (^max  "  K 

dK  dK 

dx 

Kt nax - 5 

ds 

for 

x~r(sd))<S<Sd 

dK 

(36) 
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K,ow=< 


K(sb)-C-rs  for 
ds 


ds 


K„ 


Sb<S<  —  fcmin  -  k(s„)) 
dK 

for  (Kmm  -K(sb))<s<^~ (Kmm  -  k(s, )) 

dK  dK 


(37) 


Knn  for  -J-  k„in  -K(sd))<s<sd 

ds  dK 


In  general,  steps  4-6  generate  a  curvature  profile  that  either  follows  the  curvature 
constraints  or  transitions  between  them.  These  steps  are  described  in  detail;  however,  it 
is  important  to  realize  that  the  end  result  is  simply  a  curvature  profile  that  satisfies 
Equation  35. 


4.  If  the  hazard  avoidance  maneuver  has  caused  a  UGV  to  deviate  to  the  left  of  the 
desired  nominal  path,  the  path  resumption  maneuver  generates  a  curvature  profile  that 
starts  by  following  the  lower  curvature  constraint,  Klow .  This  results  in  a  path 
resumption  maneuver  that  tends  toward  the  right  and  is  thus  likely  to  minimize 
overall  path  deviation: 


k/ow(s) 

<  k3(s-As)+  —  kmax 


for  sb  <  s  <  sb  +  As 

for  sb+  As  <  s  <  sd 


(38) 


where  A.s’  is  the  interval  defined  as: 


A  S  =  ^L^ 


(39) 


where  n  is  a  fixed  integer  that  represents  the  number  of  subdivisions  of  the  curvature 
profile.  If  the  UGV  is  on  the  right  side  of  the  nominal  desired  path,  then  the 
curvature  profile  begins  by  following  the  upper  curvature  constraint,  Khi  h.  This 
results  in  a  path  resumption  maneuver  that  tends  toward  the  left: 


K-high  (  S  ) 


for 


sh<  s  <  sb  +  As 


K3  (s  -As)-  —  kmax  for  sh+As<s< 
v 


(40) 


5.  If  at  any  point  the  curvature  profile  of  the  path  resumption  maneuver  is  less  than  the 
lower  curvature  constraint,  k3(s)<  k1ow(s),  then  the  path  resumption  maneuver 
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curvature  is  set  equal  to  the  lower  curvature  constraint,  K3(s)=Khw(s).  Similarly,  if 
k3(s)>  Khrjh(s)  then  the  path  resumption  maneuver  curvature  is  set  equal  to  the  upper 
curvature  constraint,  K3(s)=Khigh(s).  This  ensures  the  curvature  profile  does  not 
violate  the  constraints  imposed  by  the  dynamic  trajectory  space  and  that  the  final 
curvature  of  the  path  resumption  maneuver,  K3(sd)  is  equal  to  the  final  curvature  of 
the  nominal  path,  Kx  (sc ). 


6.  If  the  area  under  the  nominal  desired  trajectory’s  curvature  profile  is  within  a 
reasonable  tolerance,  eK ,  of  the  area  under  the  hazard  avoidance  maneuver  plus  the 
path  resumption  maneuver  curvature  profiles: 


J  A,  {s)ds  -  | k2  (s)ds  +  J 


K3(s)ds 


<  £ 


(41) 


then  the  algorithm  continues  to  step  7.  Otherwise  As  is  increased  and  steps  4-5  are 
repeated.  This  iterative  process  searches  the  possible  curvature  profiles  that 
constitute  a  path  resumption  maneuver  while  maintaining  k  =  /cmax . 


If  the  entire  path  resumption  maneuver  equals  either  Khigh  or  Klow  before  Equation  41 
is  satisfied,  the  distance  for  path  resumption  maneuver,  s*  is  not  large  enough.  In 
this  case,  As  is  reset  to  its  original  value,  s<*  is  increased  by  a  set  length,  and  the 
algorithm  returns  to  step  3. 

From  the  path  resumption  maneuver  curvature  profile,  heading  and  position  profiles 
can  be  generated  using  Equation  30.  At  this  point  there  is  no  guarantee  that  the  position 
of  the  path  resumption  maneuver  matches  the  nominal  desired  path  (see  Figure  16).  If 
they  do  not  match,  sc  and  Sd  are  modified  based  on  the  Euclidian  distance  from 
(x(sc )  J  (sc )),  to  (x(sd  )  y  (sd  )). . 
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Figure  16.  Illustration  of  curvature  matching  method  path. 


7.  An  acceptable  threshold  for  the  final  position  error,  e,  is  defined.  If  the  total  position 
error,  etotal  =  V fo  (\  h  A  (a  )f  +  0;,  (a  )~  A  (a  )f  .  lies  within  a  circle  of  radius  e, 
then  the  algorithm  is  complete.  If  not,  sc  and  Sd  are  adjusted  as: 

ScM  =  A,  -Ktelon) 


=  A,  -Kkhu) 


(42) 


where  kc  and  kd  are  static  positive  gains  and  e/on  and  eiat  are  the  longitudinal  and 
lateral  error  respectively  and  are  given  as: 

eia,  =  fe  (sd  )-  *1  (a  ))sin  (a  )+  G;,  (a  )-  A  (a  ))eos  0,  (sc )  (43) 

eion  =  (A  (a.  )“  x3  (A  ))eos  0,  (sc )+  (j,  (a  )-  A  (a  ))sin  °i  (a  )  (44) 

Due  to  the  fact  that  the  equations  of  motion  are  coupled  and  nonlinear  (see  Equation 
31)  algorithm  convergence  cannot  be  guaranteed.  However,  the  convergence  properties 
have  been  studied  numerically  and  have  yielded  excellent  results  (Spenko,  2005).  A  ten 
thousand  trial  simulation  using  a  PHI  1.5  GHz  computer  showed  the  curvature  matching 
method  generating  a  path  with  a  median  time  of  10  ms  and  a  mean  time  of  44  ms,  which 
indicate  the  algorithm  is  sufficiently  fast  for  use  in  high-speed  situations. 

Figure  17  shows  an  example  path  resumption  maneuver  generated  using  the  curvature 
matching  method.  Note  that  the  nominal  path’s  curvature  and  heading  and  the  path 
resumption  curvature  and  heading  profiles  are  identical  at  points  sc  and  Sd  (upper  left  and 
upper  right  subplots),  and  points  sc  and  s,i  are  coincident  along  the  path  (lower  subplot). 
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Figure  17.  Example  of  curvature  matching  method. 
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4.  Simulation  and  Experimental  Results 

4.1.  Experimental  System  Description 

Experimental  trials  were  conducted  on  the  Autonomous  Rough  Terrain  Experimental 
System  (ARTEmiS);  see  Figure  18.  ARTEmiS  is  a  front-steer  rear- wheel  drive  UGV  that 
measures  0.88  m  long,  0.61  m  wide,  and  0.38  m  high.  It  has  a  0.56  m  wheelbase  and 
0.25  m  diameter  pneumatic  tires.  It  is  equipped  with  a  2.5  Hp  Zenoah  G2D70  gasoline 
engine,  Crossbow  AHRS-400  inertial  navigation  system  (INS),  Novatel  differential 
global  positioning  system  (DGPS)  capable  of  0.2  meter  resolution  (circular  error 
probable),  Futaba  S5050  servos  for  steering,  brakes,  and  throttle,  and  a  PHI  700  MHz 
PC  104  computer.  ARTEmiS  is  not  equipped  with  forward-looking  range  sensors. 
Instead,  using  knowledge  of  ARTEmiS’  position,  hazard  locations  are  only  revealed  once 
they  are  within  the  range  of  a  “virtual  sensor.”  Simulations  were  conducted  using  a 
model  of  ARTEmiS  and  the  commercial  software  package  MSC.ADAMS/Car.  The 
steering  angle  and  throttle  were  controlled  using  a  PD  control.  For  the  steering  angle,  the 
gain  was  inversely  proportional  to  the  vehicle  longitudinal  velocity.  Sufficient  path 
following  results  were  obtained  using  previously  developed  algorithms  (Canudas  de  Wit, 
Siciliano,  &  Bastin,  1996). 


27 


Accepted  to  the  Journal  of  Field  Robotics 


Inertial 
Navigation 
System 
(Hidden) 

Onboard 
Computer 

Pneumatic 
Tires 

Outriggers 

Emergency 
Kill  Switch 

Figure  18.  Diagram  of  ARTEmiS. 

Because  ARTEmiS  exhibits  only  slight  oversteer,  for  the  purpose  of  the  simulations 
and  experiments  presented  in  this  chapter  the  steering  constraints  were  considered  to  be 
derived  from  a  neutral- steered  vehicle.  Also,  note  that  the  center  of  mass  of  ARTEmiS 
does  not  bisect  the  track  width  of  the  vehicle.  Thus,  the  rollover  constraints  are  not 
symmetric  about  zero  curvature. 

4.2.  Validation  of  Hazard  Avoidance  Maneuver  Algorithm 

The  hazard  avoidance  maneuver  algorithm  was  validated  through  both  simulation  and 
experimental  analysis.  Over  80  hours  of  experimental  data  was  collected  on  a  variety  of 
terrain  surfaces,  profiles,  and  conditions,  at  speeds  ranging  from  3.0-9. 0  m/s.  This 
section  provides  results  from  five  experiments. 

For  each  experiment,  ARTEmiS  was  placed  in  an  initial  starting  location,  (x0,  y0 ),  and 
commanded  to  follow  a  nominal  desired  trajectory,  Tnominal ,  with  a  corresponding  path, 
x n0minai  •  Hazards  consisted  of  traffic  cones  placed  in  various  configurations.  The  range 
of  the  sensor  varied  among  experiments  from  12  m  to  20  m  (21  to  35  times  the  vehicle 
wheelbase).  Other  experiments  were  conducted  to  investigate  the  effects  of  a  reduced 
sensor  range  on  resulting  hazard  avoidance  maneuvers.  (Due  to  length  constraints  the 
results  are  not  included  here.)  As  expected  it  was  found  that  as  the  sensor  range  is 
reduced,  the  resulting  hazard  avoidance  maneuvers  are  usually  more  severe  and 
performed  at  lower-speeds  than  similar  experiments  conducted  with  longer-range  sensors. 
Once  a  hazard  was  in  range  it  was  assumed  that  the  hazard  geometry  was  known.  All 
experiments  used  the  curvature  matching  method  to  generate  a  path  resumption 
maneuver.  All  experiments  also  used  the  maneuver  selection  cost  function  given  in 
Equation  29  with  Kx  <  K2  unless  otherwise  noted. 


28 


Accepted  to  the  Journal  of  Field  Robotics 


4.3.  Multiple  Hazard  Simulation  and  Experimental  Results 

Results  from  two  experimental  trials  are  presented  that  illustrate  the  ability  of  the 
algorithm  to  avoid  multiple  hazards.  This  section  also  contains  simulation  results  for 
comparison  to  one  of  the  experimental  trials. 


Figure  19  shows  three  “snapshot”  subplots  of  the  GPS  trace  from  an  experiment  for 
high  speed  avoidance  of  two  hazards.  The  experiment  was  performed  on  a  field  of  mixed 
grass  and  dirt,  at  a  desired  velocity  of  6.0  m/s.  The  nominal  desired  path  was  a  100  m 
long  straight  path.  ARTEmiS  detected  the  first  hazard  at  x  =  16.4  m  .  This  is  shown  in 
the  top  subplot  of  Figure  19.  At  this  point  a  hazard  avoidance  maneuver  was  executed. 
ARTEmiS  followed  the  modified  path  until  a  second  hazard  was  detected  at  x  =  43.2  m . 
This  is  shown  in  the  middle  subplot  of  Figure  19.  A  second  maneuver  was  then  executed 
and  ARTEmiS  successfully  resumed  the  nominal  path,  as  shown  in  the  lower  section  of 
Figure  19. 
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Figure  19.  Hazard  avoidance  maneuvers  executed  for  multiple  hazards. 


Figure  20  shows  the  trajectory  spaces  at  the  instant  that  the  first  hazard  was  detected. 
An  x  marks  ARTEmiS’  location  in  the  trajectory  space.  Here,  ARTEmiS  modified  its 
trajectory  from  T0  =(6.0,0.00)  to  zf  =(6.0, -0.03),  i.e.  it  executed  a  sharp  turn  to  avoid 
the  hazard. 


29 


Accepted  to  the  Journal  of  Field  Robotics 


Figure  20.  Trajectory  space  when  the  first  hazard  was  detected. 

Figure  21  compares  experimental  and  simulated  GPS  traces  for  these  experiments. 
The  top  subplot  displays  the  simulation  results  and  the  lower  subplot  shows  the 
experimental  results.  The  two  results  are  quite  similar,  though  the  simulation  generated  a 
slightly  different  maneuver  than  the  experimental  system  for  the  second  hazard.  This  is 
due  to  differences  in  ARTEmiS’  position  when  the  second  hazard  was  identified.  This 
can  be  attributed  to  position  estimation  and  path  tracking  errors  that  are  present  in  the 
experimental  system. 


Figure  21.  Comparison  of  simulation  (top)  and  experimental  (bottom)  results. 

Path  tracking  errors  in  the  experimental  system  were  due  to  position  estimation  errors 
and  mechanical  limitations  of  ARTEmiS ’s  steering  mechanism,  which  are  backdrivable 
and  slightly  underpowered.  Thus  terrain  roughness  caused  substantial  disturbances  to  the 
steering  system. 

4.4.  Sloped  Terrain  Experimental  and  Simulation  Results 

An  important  property  of  the  hazard  avoidance  algorithm  is  its  ability  to  account  for 
the  effects  of  terrain  inclination.  Here  the  results  of  two  experimental  trials  are 
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compared.  The  experiments  were  identical  except  that  the  first  was  performed  on  flat 
terrain  and  the  second  was  on  terrain  with  a  15-18°  slope  with  the  fall  line  perpendicular 
to  the  initial  direction  of  vehicle  travel  (see  Figure  22). 


Figure  22.  Image  of  ARTEmiS  sloped  terrain  experiment. 


The  experiments  were  performed  at  a  desired  speed  of  8.0  m/s.  The  nominal  desired 
path  for  each  trial  was  a  100  m  long  curved  path.  For  both  experiments  ARTEmiS 
traversed  the  nominal  desired  path  until  it  detected  a  hazard  at  s  =  16.4  m.  At  the  time 
the  hazard  was  identified,  ARTEmiS  selected  a  hazard  avoidance  maneuver  of 
Tf  =(8.0,0.12)  on  flat  terrain.  On  sloped  terrain  ARTEmiS  selected  rf  =(7.0, -0.06). 
This  is  due  to  the  fact  that  on  sloped  terrain,  zf  =(8.0,0.12)  was  not  deemed  to  be  a 
dynamically  admissible  maneuver  due  to  the  effects  of  terrain  inclination.  This  can  be 
seen  in  Figure  23. 


Flat  Terrain 


Sloped  Terrain 


Figure  23.  Trajectory  space  comparison  for  flat  and  sloped  terrain. 


GPS  traces  of  the  resulting  paths  are  compared  in  Figure  24.  There  is  significant  path 
tracking  overshoot  in  the  flat  terrain  case  due  to  steering  servo  rate  limitations.  However, 
this  experiment  illustrates  the  effect  of  terrain  inclination  on  maneuver  selection,  and 
shows  that  the  algorithm  results  in  a  dynamically  admissible  maneuver  even  on  steeply 
sloped  terrain. 
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Figure  24.  Hazard  avoidance  maneuver  enacted  on  flat  and  sloped  terrain. 

4.5.  Rough  Terrain  Experimental  Results 

Experiments  on  rough  terrain  were  performed  at  Minute  Man  National  Historic  Park. 
The  terrain  consisted  of  a  bumpy,  uncut  grass  field.  Physical  terrain  features  tended  to  be 
on  the  order  of  one-half  the  wheel  radius.  Figure  25  illustrates  the  roughness  of  the 
terrain  by  comparing  experimentally-measured  UGV  vertical  acceleration  measured  on 
both  smooth  and  rough  terrain  at  the  experiment  site.  Data  was  gathered  while 
ARTEmiS  traveled  at  7  m/s. 


Rough  Terrain 
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Figure  25.  Vertical  acceleration  comparison  on  rough  and  flat  terrain. 


Figure  26  shows  the  experimental  site.  The  nominal  desired  path  is  a  100  m  long 
straight  path.  ARTEmiS  is  pictured  at  the  start  of  the  path.  The  goal  location  is 
obstructed  from  view  by  the  hazard.  The  hazard  consists  of  a  cluster  of  tall  brushes,  and 
small  trees. 
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Figure  26.  Rough  terrain  experimental  setup. 


Figure  27  shows  three  “snapshot”  subplots  of  the  experiment.  The  experiment  was 
performed  at  a  speed  of  7.0  m/s.  ARTEmiS  detected  the  first  hazard  at  x  =  10.4  m .  This 
is  shown  in  the  top  subplot  of  Figure  27.  At  this  point  hazard  avoidance  and  path 
resumption  maneuvers  were  executed,  as  shown  in  the  middle  subplot  of  Figure  27.  The 
lower  section  of  Figure  27  shows  the  completed  path. 
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Figure  27.  Rough  terrain  experimental  results. 


Figure  28  shows  the  trajectory  space  at  the  time  the  hazard  was  detected.  The 
dynamic  rollover  limits  included  an  empirically-determined  “safety  margin”  to 
compensate  for  the  effects  of  terrain  roughness.  When  the  hazard  was  detected, 
ARTEmiS  modified  its  trajectory  from  T0  =  (7. 0,0. 00 )  to  T f  =(7.0,0.03). 
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Figure  28.  Rough  terrain  trajectory  space. 

This  experiment  demonstrates  that  the  proposed  hazard  avoidance  algorithm  can  be 
applied  in  to  UGVs  operating  at  high  speeds  on  rough  terrain.  These  conditions  are 
expected  to  be  similar  to  actual  operating  conditions  for  many  practical  applications. 

5.  Conclusions 

This  paper  has  presented  an  algorithm  for  hazard  avoidance  for  high-speed  unmanned 
ground  vehicles  operating  on  rough,  natural  terrain.  The  algorithm  accounts  for  dynamic 
effects  such  as  vehicle  sideslip,  rollover,  and  over/understeer,  as  well  as  vehicle  steering 
dynamics,  drive  train  properties,  terrain  geometry,  and  vehicle/terrain  interaction.  The 
method  is  computationally  efficient  (operating  on  the  order  of  milliseconds),  and  thus 
suitable  for  on-board  implementation.  Extensive  simulation  and  experimental  results 
have  been  presented  that  demonstrate  the  algorithm’s  effectiveness.  The  hazard 
avoidance  algorithm  based  on  the  trajectory  space  is  only  one  of  many  that  could  be 
implemented,  and  future  work  focuses  on  expanding  this  area. 
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